function [ smoothMat ] = SmoothnessMatrix(penalty)
    mask = [ 0 1 0 ; 1 0 1 ; 0 1 0 ];
    
    matRows = [];
    matCols = [];
    matData = [];
    
    bigPenalty = BigPenalty(penalty);
    
    [ y x ] = meshgrid(1 : size(penalty, 1), 1 : size(penalty, 2));

    for i = 1 : numel(mask)
        if mask(i)
            [ row col ] = ind2sub(size(mask), i);
            row = row - 2;
            col = col - 2;
            
            bigX = 2 * x + col;
            bigY = 2 * y + row;
            
            curX = x + col;
            curY = y + row;
            
            goodLocs = find((curX >= 1) .* (curX <= size(penalty, 2)) .* (curY >= 1) .* (curY <= size(penalty, 1)));
            
            outputPixInd = sub2ind(size(penalty), y(goodLocs), x(goodLocs));             
            inputPixInd = sub2ind(size(penalty), curY(goodLocs), curX(goodLocs));            
            
            bigPenaltyInd = sub2ind(size(bigPenalty), bigY(goodLocs), bigX(goodLocs));
            
            matRows = [ matRows; outputPixInd ];
            matCols = [ matCols;  inputPixInd ];
            matData = [ matData; bigPenalty(bigPenaltyInd) ];
        end
    end
    smoothMat = sparse(matRows, matCols, matData, numel(penalty), numel(penalty));
    rowSums = sum(smoothMat, 2);
    smoothMatDiag = spdiags(rowSums, 0, size(smoothMat, 1), size(smoothMat, 2));    
    smoothMat = smoothMat - smoothMatDiag;
end

function bigPenalty = BigPenalty(penalty)
   vertMeans = imfilter(penalty, [ 1 ; 1 ] / 2, 'symmetric', 'full');     
   horizMeans = imfilter(penalty, [ 1  1 ] / 2, 'symmetric', 'full');
   
   bigPenalty = zeros(2 * size(penalty) + [ 1 1 ]);
   bigPenalty(1 : 2 : end, 2 : 2 : end) = vertMeans;
   bigPenalty(2 : 2 : end, 1 : 2 : end) = horizMeans;
end